#include <math.h>
#include <stdio.h>

//地球半径
#define R 6371393.0
#define PI 3.1415926

//#ifndef DEBUG
//#define DEBUG
//#endif // !DEBUG

/*
target position: (des_lan des_lon)
current positiob: (cur_lan,cur_lon)
return value theta
0对应地磁北极
0 --> 360: Turn  clockwise from north;
返回数值单位为度
lan 纬度
lon 经度
*
|
|
|
|
* -----------*
 */
double get_target_direction(double des_lan, double des_lon, double cur_lan, double cur_lon)
{
	double theta_t = 0.0;
	double theta = 0.0;
	double des_lan_r = des_lan * PI / 180.0;
	double cur_lan_r = cur_lan * PI / 180.0;
	double des_lon_r = des_lon * PI / 180.0;
	double cur_lon_r = cur_lon * PI / 180.0;
	double L1 = R * (des_lan_r - cur_lan_r);
	double L2 = R * cos(cur_lan_r) * (des_lon_r - cur_lon_r);
#ifdef DEBUG
	printf("L1=%lf,L2=%lf\n", L1, L2);
#endif // DEBUG
	theta_t = atan(fabs(L1 / L2));

	if (L1 >= 0.0 && L2 >= 0.0) //I
	{
		theta = PI / 2.0 - theta_t;
#ifdef DEBUG
		printf("I\n");
#endif // DEBUG
	}
	else if (L1 >= 0.0 && L2 < 0.0) //II
	{
		theta = 3.0 * PI / 2.0 + theta_t;
#ifndef DEBUG
		printf("II\n");
#endif // !DEBUG
	}
	else if (L1 < 0.0 && L2 < 0.0) //III
	{
		theta = 3.0 * PI / 2.0 - theta_t;
#ifndef DEBUG
		printf("III\n");
#endif // !DEBUG
	}
	else if (L1 <= 0.0 && L2 > 0.0) //IV
	{
		theta = PI / 2.0 + theta_t;
#ifndef DEBUG
		printf("IV\n");
#endif // !DEBUG
	}
	else
	{
		printf("get direction fault!\n");
	}
	theta = theta * 180.0 / PI;
	return theta;
}

static double haver_sin(double theta)
{
	double v = sin(theta / 2.0);
	return v * v;
}

/*
target position: (des_lan des_lon)
current positiob: (cur_lan,cur_lon)

return value is the distance bettwen this two positions;
计算目标点与当前点的距离单位为m
 */
double get_distance_err(double des_lan, double des_lon, double cur_lan, double cur_lon)
{
	//用haversine公式计算球面两点间的距离。
	//经纬度转换成弧度
	double des_lan_r = des_lan * PI / 180.0;
	double cur_lan_r = cur_lan * PI / 180.0;
	double des_lon_r = des_lon * PI / 180.0;
	double cur_lon_r = cur_lon * PI / 180.0;
	//差值
	double v_lon = fabs(des_lon_r - cur_lon_r);
	double v_lan = fabs(des_lan_r - cur_lan_r);

	//h is the great circle distance in radians, great circle就是一个球体上的切面，它的圆心即是球心的一个周长最大的圆。
	double h = haver_sin(v_lan) + cos(des_lan_r) * cos(cur_lan_r) * haver_sin(v_lon);

	double distance = 2 * R * sin(sqrt(h));

	return distance;
}

int cal_main(int argc, char const *argv[])
{
	double des_lan = 0.0;
	double des_lon = 0.0;
	double cur_lan = 0.0;
	double cur_lon = 0.0;
	printf("input target position(lan,lon)\n");
	scanf("%lf,%lf", &des_lan, &des_lon);
	printf("input current position(lan,lon)\n");
	scanf("%lf,%lf", &cur_lan, &cur_lon);
	double theta = get_target_direction(des_lan, des_lon, cur_lan, cur_lon);
	double distance = get_distance_err(des_lan, des_lon, cur_lan, cur_lon);
	printf("theta=%lf,distance=%lf\n", theta, distance);
	getchar();
	return 0;
}

//void test()
//{
	//while(1)
	//{
		//get_cur_gps();
		//get_imu();
		//dir = 	get_target_direction();
		//dis = get_target_distance();
		//rot(dir*k1);
		//move(dis);
		//if(dis < 2)
		//{
			//printf("到达!!!\n");
		//}
	//}
//}
